#include "3dApp.h"

using namespace std;


	
void sortBad(float *root)
{
	if(root[1]<root[0]){
		float tmp=root[0]; root[0]=root[1];
		root[1]=tmp;
	}
	if(root[2]<root[0]){
		float tmp=root[0];root[0]=root[2];
		root[2]=tmp;
	}
	if(root[2]<root[1]){
		float tmp=root[1];root[1]=root[2];
		root[2]=tmp;
	}
}

void computeRoots (float cov[][3], float root[3])
{
	float a00 = (float)cov[0][0];
	float a01 = (float)cov[0][1];
	float a02 = (float)cov[0][2];
	float a11 = (float)cov[1][1];
	float a12 = (float)cov[1][2];
	float a22 = (float)cov[2][2];
	float c0 = a00*a11*a22 + 2.0*a01*a02*a12 - a00*a12*a12 - a11*a02*a02 - a22*a01*a01;
	float c1 = a00*a11 - a01*a01 + a00*a22 - a02*a02 + a11*a22 - a12*a12;
	float c2 = a00 + a11 + a22;
	float c2Div3 = c2*1/3.0;
	float aDiv3 = (c1 - c2*c2Div3)*1/3.0;
	if (aDiv3 > 0.0) { aDiv3 = 0.0; }
	float mbDiv2 = 0.5*(c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));
	float q = mbDiv2*mbDiv2 + aDiv3*aDiv3*aDiv3;
	if (q > 0.0) { q = 0.0; }
	float magnitude = sqrt(-aDiv3);
	float angle = atan2(sqrt(-q),mbDiv2)*1/3.0;
	float cs = cos(angle);
	float sn = sin(angle);
	root[0] = c2Div3 + 2.0*magnitude*cs;
	root[1] = c2Div3 - magnitude*(cs + sqrt(3.0)*sn);
	root[2] = c2Div3 - magnitude*(cs - sqrt(3.0)*sn);
	sortBad(root);
// Sort the roots here to obtain root[0] <= root[1] <= root[2].
}

float lengthBad(float a, float b, float c){
	float length = sqrt(pow(a,2)+pow(b,2)+pow(c,2));
	return length;
}

inline
void eigenVectorCalc(float t[], float e[][3], float cov[][3]){
	int i,j;
	float a=cov[1][0],b=cov[2][0],c=cov[2][1];
	float x=cov[0][0],y=cov[1][1],z=cov[2][2];
	for(i=0;i<3;i++)
	{
		e[i][0]=1;
		e[i][1]=((t[i]-x)*c+b*a)/((t[i]-y)*b+a*c);
		e[i][2]=((t[i]-x)*c+b*a)/(b*c+a*(t[i]-z));
		float l=lengthBad(e[i][0],e[i][1],e[i][2]);
		for(j=0;j<3;j++)
			e[i][j]/=l;
	}
}